/********************************************************************
 *
 *      Filename: Quaternion.cpp
 *   Description: See Quaternion.h
 *        Author: [kb]ypp
 *    Created at: February 26, 2005
 * Last modified: November 5, 2005
 *
 ********************************************************************/

#include "Quaternion.h"
#include "MathUtil.h"
#include "Vector3.h"
#include "Matrix3x3.h"
#include "../core/Log.h"
#include <cassert>

//! Does not perform any initialization
//!
quat::quat()
{}

quat::quat( const quat &q )
	: w( q.w ), x( q.x ), y( q.y ), z( q.z )
{}

quat::quat( float qw, float qx, float qy, float qz )
	: w( qw ), x( qx ), y( qy ), z( qz )
{}

/*
// Initialize the quat by given
// components: w and vector v( x, y, z )
quat::quat( float qw, Vector3 qv )
	: w( qw ), x( qv.x ), y( qv.y ), z( qv.z )
{}
*/

const quat& quat::operator=( const quat &q )
{
	w = q.w;
	x = q.x;
	y = q.y;
	z = q.z;
	return *this;
}

const quat operator*( const quat &q1, const quat &q2 )
{
	return quat( q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
				 q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
				 q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x,
				 q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w );
}

bool operator==( const quat &q1, const quat &q2 )
{
	return fabs( q1.w - q2.w ) < EPSILON &&
           fabs( q1.x - q2.x ) < EPSILON &&
           fabs( q1.y - q2.y ) < EPSILON &&
		   fabs( q1.z - q2.z ) < EPSILON;
}

bool operator!=( const quat &q1, const quat &q2 )
{
	return !( q1 == q2 );
}

//! Produces a unit quaternion (or zero quaternion + log warning)
//!
void quat::normalize()
{
	float mag = sqrt( w * w + x * x + y * y + z * z );
	if ( mag > EPSILON ) { // Avoid division-by-zero
		float oneOverMag = 1.0 / mag;
		w *= oneOverMag;
		x *= oneOverMag;
		y *= oneOverMag;
		z *= oneOverMag;
	} else {
		LOG_WARNING( "Attempt to normalize() zero quaternion" );
		w = x = y = z = 0.0;
	}
}

const mat3 quat::toMatrix() const
{
	const float wx = w * x;
	const float wy = w * y;
	const float wz = w * z;
	const float xx = x * x;
	const float xy = x * y;
	const float xz = x * z;
	const float yy = y * y;
	const float yz = y * z;
	const float zz = z * z;
	
	return mat3( 
		1.0 - 2.0*( yy + zz ),       2.0*( xy - wz ),       2.0*( xz + wy ),
		      2.0*( xy + wz ),  1.0 - 2.0*(xx + zz ),       2.0*( yz - wx ),
		      2.0*( xz - wy ),       2.0*( yz + wx ), 1.0 - 2.0*( xx + yy )
	);
}

//! This is the quaternion with the opposite rotation. Defined as follows:
//! q* = [ w -v ] = [ w ( -x -y -z ) ]
const quat conj( const quat &q )
{
	return quat( q.w, -q.x, -q.y, -q.z );
}

// @note t must be in range [0.0; 1.0]
const quat slerp( const quat &q0, const quat &q1, float t )
{
	if ( t <= 0.0 )
		return q0;
	else if ( t >= 1.0 )
		return q1;
	
	quat q( q1 );
	// cosTheta = dot( q0, q1 )
	float cosTheta = q0.w * q1.w + q0.x * q1.x + q0.y * q1.y + q0.z * q1.z;
	if ( cosTheta < 0.0 ) {
		q.w = -q1.w;
		q.x = -q1.x;
		q.y = -q1.y;
		q.z = -q1.z;
		cosTheta = -cosTheta;
	}
	// Both quaternions assumed to have unit length.
	// Thus, cosTheta must be <= 1.0
	assert( cosTheta < 1.0 + EPSILON );
	
	float k0, k1;
	
	if ( cosTheta > 1.0 - EPSILON ) {
		// Quaternions "very close" each other.
		// Using linear interpolation
		k0 = 1.0 - t;
		k1 = t;
	} else {
		float sinTheta = sqrt( 1.0 - cosTheta * cosTheta );
		float theta = atan2( sinTheta, cosTheta );
		float oneOverSinTheta = 1.0 / sinTheta; // sinTheta != 0.0 for sure
		k0 = sin( ( 1.0 - t ) * theta ) * oneOverSinTheta;
		k1 = sin( t * theta ) * oneOverSinTheta;
	}
	// Interpolate
	return quat( k0 * q0.w + k1 * q.w,
				 k0 * q0.x + k1 * q.x,
				 k0 * q0.y + k1 * q.y,
				 k0 * q0.z + k1 * q.z );
}

//! q = [ cos(theta/2) ( sin(theta/2) 0 0 ) ]
//! Counter-clockwise rotation angle `theta' given in radians.
const quat quat::rotateX( float theta )
{
	const float halfTheta = 0.5 * theta;
	return quat( cos( halfTheta ), sin( halfTheta ), 0.0, 0.0 );
}

//! q = [ cos(theta/2) ( 0 sin(theta/2) 0 ) ]
//! Counter-clockwise rotation angle `theta' given in radians.
const quat quat::rotateY( float theta )
{
	const float halfTheta = 0.5 * theta;
	return quat( cos( halfTheta ), 0.0, sin( halfTheta ), 0.0 );
}

//! q = [ cos(theta/2) ( 0 0 sin(theta/2) ) ]
//! Counter-clockwise rotation angle `theta' given in radians.
const quat quat::rotateZ( float theta )
{
	const float halfTheta = 0.5 * theta;
	return quat( cos( halfTheta ), 0.0, 0.0, sin( halfTheta ) );
}

//! q(theta, n) = [ cos(theta/2) ( nx*sin(theta/2) ny*sin(theta/2) nz*sin(theta/2) ) ]
//! Rotation axis `axis' assumed to be a unit vector.
//! Counter-clockwise rotation angle `theta' given in radians.
const quat quat::rotateAxis( const vec3 &axis, float theta )
{
	const float halfTheta = 0.5 * theta;
	const float s = sin( halfTheta );
	
	return quat( cos( halfTheta ), axis.x * s, axis.y * s, axis.z * s );
}
